{# ----------------------------------------------------------------------------
 # SymForce - Copyright 2022, Skydio, Inc.
 # This source code is under the Apache 2.0 license found in the LICENSE file.
 # ---------------------------------------------------------------------------- #}

{%- import "../util/util.jinja" as util with context -%}

#pragma once

#include <ostream>
#include <random>
#include <vector>
#include <Eigen/Dense>

#include <sym/ops/storage_ops.h>

namespace sym {

{% if doc %}
/**
 * Autogenerated C++ implementation of {{ cls }}.
 *
{% for line in doc.split('\n') %}
 *{{ ' {}'.format(line).rstrip() }}
{% endfor %}
 */
{% endif %}
template <typename ScalarType>
class {{ cls.__name__ }} {
 public:
  // Typedefs
  using Scalar = ScalarType;
  using Self = {{ cls.__name__ }}<Scalar>;
  using DataVec = Eigen::Matrix<Scalar, {{ ops.StorageOps.storage_dim(cls) }}, 1>;

  // Construct from {% for arg, _ in storage_order %}{% if not loop.last %}{{ arg }}, {% else %}and {{ arg }}{% endif %}{% endfor %}.
  {{ cls.__name__ }}(
        {% for arg, size in storage_order %}
        const {% if size == 1 %}Scalar{% else %}Eigen::Matrix<Scalar, {{ size }}, 1>&{% endif %} {{ arg }}{% if not loop.last %}, {% endif %}
        {% endfor %}
  ) : {{ cls.__name__ }}((Eigen::Matrix<Scalar, sym::StorageOps<Self>::StorageDim(), 1>() <<
        {% for arg, _ in storage_order %}{{ arg }}{% if not loop.last %}, {% endif %}{% endfor %}).finished()) {}

  // Construct from data vec
  explicit {{ cls.__name__ }}(const DataVec& data) : data_(data) {}

  // Access underlying storage as const
  inline const DataVec& Data() const {
      return data_;
  }

  // --------------------------------------------------------------------------
  // StorageOps concept
  // --------------------------------------------------------------------------

  static constexpr int32_t StorageDim() {
    return sym::StorageOps<Self>::StorageDim();
  }

  void ToStorage(Scalar* const vec) const {
    return sym::StorageOps<Self>::ToStorage(*this, vec);
  }

  static {{ cls.__name__ }} FromStorage(const Scalar* const vec) {
    return sym::StorageOps<Self>::FromStorage(vec);
  }

  // --------------------------------------------------------------------------
  // Camera model methods
  // --------------------------------------------------------------------------
  {% for spec in specs['CameraOps'] -%}
  {{ util.print_docstring(spec.docstring) | indent(2) }}
  {{ util.function_declaration(spec, is_declaration=True) }} const;
  {% endfor %}

  // --------------------------------------------------------------------------
  // General Helpers
  // --------------------------------------------------------------------------

  bool IsApprox(const Self& b, const Scalar tol) const {
    // isApprox is multiplicative so we check the norm for the exact zero case
    // https://eigen.tuxfamily.org/dox/classEigen_1_1DenseBase.html#ae8443357b808cd393be1b51974213f9c
    if (b.Data() == DataVec::Zero()) {
      return Data().norm() < tol;
    }

    return Data().isApprox(b.Data(), tol);
  }

  template <typename ToScalar>
  {{ cls.__name__ }}<ToScalar> Cast() const {
    return {{ cls.__name__ }}<ToScalar>(Data().template cast<ToScalar>());
  }

  bool operator==(const {{ cls.__name__ }}& rhs) const {
    return data_ == rhs.Data();
  }

 protected:
  DataVec data_;
};

// Shorthand for scalar types
{% for scalar in scalar_types %}
using {{ cls.__name__ }}{{ scalar[0] }} = {{ cls.__name__ }}<{{ scalar }}>;
{% endfor %}

// Print definitions
{% for scalar in scalar_types %}
std::ostream& operator<<(std::ostream& os, const {{ cls.__name__ }}<{{ scalar }}>& a);
{% endfor %}

}  // namespace sym

// Externs to reduce duplicate instantiation
{% for scalar in scalar_types %}
extern template class sym::{{ cls.__name__ }}<{{ scalar }}>;
{% endfor %}

// Concept implementations for this class
#include "./ops/{{ camelcase_to_snakecase(cls.__name__) }}/storage_ops.h"
#include "./ops/{{ camelcase_to_snakecase(cls.__name__) }}/lie_group_ops.h"
#include "./ops/{{ camelcase_to_snakecase(cls.__name__) }}/group_ops.h"
